
#include "Box.h"

#include <windows.h>
#include <GL/gl.h>


void Box::init(void){
	
	std::cout << vel[0] << " " <<vel[1] << " " <<vel[2] << std::endl;
	//std::cout << "OH GOD WHERE AM I?!?!?!";
	
	colors = new float*[6];
	for (int i=0; i<6; i++) colors[i] = new float[3];
	colors[0][0]=1.f; colors[0][1]=0.f; colors[0][2]=0.f; 
	colors[1][0]=0.f; colors[1][1]=1.f; colors[1][2]=0.f; 
	colors[2][0]=0.f; colors[2][1]=0.f; colors[2][2]=1.f; 
	colors[3][0]=1.f; colors[3][1]=1.f; colors[3][2]=0.f; 
	colors[4][0]=0.f; colors[4][1]=1.f; colors[4][2]=1.f; 
	colors[5][0]=1.f; colors[5][1]=0.f; colors[5][2]=1.f; 
	
}
Box::Box(void) : Body()
{
	set(1.f,1.f,1.f);
	init();
}
Box::Box(float length) : Body()
{
	set(length, length, length);
	init();
}
Box::Box(float depth, float width, float height) :	Body()
{
	//super();
	set(depth, width, height);
	init();
}

void Box::set(float depth, float width, float height)
{
	vertices = new Vector4*[8];
	for(int i=0; i<8; i++) vertices[i]=new Vector4();
	vertices[0]->set(0,-width/2.f);  vertices[0]->set(1, height/2.f);  vertices[0]->set(2,-depth/2.f);
	vertices[1]->set(0,-width/2.f);  vertices[1]->set(1, height/2.f);  vertices[1]->set(2, depth/2.f);
	vertices[2]->set(0, width/2.f);  vertices[2]->set(1, height/2.f);  vertices[2]->set(2, depth/2.f);
	vertices[3]->set(0, width/2.f);  vertices[3]->set(1, height/2.f);  vertices[3]->set(2,-depth/2.f);
	vertices[4]->set(0,-width/2.f);  vertices[4]->set(1,-height/2.f);  vertices[4]->set(2,-depth/2.f);
	vertices[5]->set(0,-width/2.f);  vertices[5]->set(1,-height/2.f);  vertices[5]->set(2, depth/2.f);
	vertices[6]->set(0, width/2.f);  vertices[6]->set(1,-height/2.f);  vertices[6]->set(2, depth/2.f);
	vertices[7]->set(0, width/2.f);  vertices[7]->set(1,-height/2.f);  vertices[7]->set(2,-depth/2.f);

	indices = new int[6*4];
	indices[0] = 0; indices[1] = 1; indices[2] = 2; indices[3] = 3;
	indices[4] = 4; indices[5] = 0; indices[6] = 3; indices[7] = 7;
	indices[8] = 7; indices[9] = 3; indices[10]= 2; indices[11]= 6;
	indices[12]= 5; indices[13]= 1; indices[14]= 0; indices[15]= 4;
	indices[16]= 5; indices[17]= 4; indices[18]= 7; indices[19]= 6;
	indices[20]= 6; indices[21]= 2; indices[22]= 1; indices[23]= 5;

	mass = depth * width * height * 1.0f;

	MOI = generateMOI(depth, width, height);
	MOIinv = MOI.inverse();
}

void Box::draw(Vector4 *camPos, Vector4 *camRot)
{
	glBegin(GL_QUADS);
	for(int i=0; i<6; i++)
	{
		glColor3f(colors[i][0], colors[i][1], colors[i][2]);
		for(int j=0; j<4; j++)
		{
			int vertNum = indices[(i*4)+j];
			Vector4 newVector = tMat.times(*vertices[vertNum]);
			glVertex3f(newVector.get(0),
					   newVector.get(1),
					   newVector.get(2));
		}
	}
	glEnd();
}

Vector4 **Box::getFaceVerts()
{
	Vector4 **v = new Vector4*[24];
	for(int i=0; i<24; i++) {
		v[i] = new Vector4(vertices[indices[i]]);
		v[i] = v[i]->plus(&pos);
	}
	return v;
}

void Box::update(float etime){
	Body::update(etime);

	Vector4 emptyHPR = Vector4();
	RK4(emptyHPR, aVel, aAcc, etime);
	rotHPR(emptyHPR[0],emptyHPR[1],emptyHPR[2]);
	//std::cout << "ori " << ori[0] << " " << ori[1] << " " << ori[2] << std::endl;
	updateMatrix();
	acc = Vector4();
	aAcc = Vector4();
	//transformXYZHRPS(emptyPos[0], emptyPos[1], emptyPos[2], emptyHPR[0],emptyHPR[1],emptyHPR[2], 1.0);


}

Matrix44 Box::generateMOI(float depth, float width, float height){
	Matrix44 newMOI =  Matrix44();
	float mScale = 1.f/mass;
	newMOI(0,0) = (1.0f/12.0f)*mass*mScale*((height * height) + (depth * depth));
	newMOI(1,1) = (1.0f/12.0f)*mass*mScale*((width * width) + (depth * depth));
	newMOI(2,2) = (1.0f/12.0f)*mass*mScale*((width * width) + (height * height));
	return newMOI;

}


